#!/usr/bin/env python

# Copyright (c) 2018 Intel Labs.
# authors: German Ros (german.ros@intel.com)
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

""" This module contains a local planner to perform
low-level waypoint following based on PID controllers. """

from collections import deque
from enum import Enum
import math
import numpy as np
from scipy.spatial import cKDTree

import carla
from agents.navigation.controller import VehiclePIDController
from agents.tools.misc import get_speed, get_acceleration, get_nearest_waypoint_index, compute_magnitude_angle
from agents.tools.misc import distance_vehicle, draw_waypoints, draw_way, compute_distance, draw_speed_with_tf


class RoadOption(Enum):
    """
    RoadOption represents the possible topological configurations
    when moving from a segment of lane to other.
    """
    VOID = -1
    LEFT = 1
    RIGHT = 2
    STRAIGHT = 3
    LANEFOLLOW = 4
    CHANGELANELEFT = 5
    CHANGELANERIGHT = 6


class LocalPlanner(object):
    """
    LocalPlanner implements the basic behavior of following a trajectory
    of waypoints that is generated on-the-fly.
    The low-level motion of the vehicle is computed by using two PID controllers,
    one is used for the lateral control
    and the other for the longitudinal control (cruise speed).

    When multiple paths are available (intersections)
    this local planner makes a random choice.
    """

    # Minimum distance to target waypoint as a percentage
    # (e.g. within 80% of total distance)

    # FPS used for dt
    FPS = 20

    def __init__(self, agent):
        """
        :param agent: agent that regulates the vehicle
        :param vehicle: actor to apply to local planner logic onto
        """
        self._vehicle = agent.vehicle
        self._sampling_resolution = agent._sampling_resolution
        self._dt = 1.0/self.FPS
        
        vehicle_physics_control = self._vehicle.get_physics_control()
        weels = vehicle_physics_control.wheels
        # self._front_centre = (weels[0].position + weels[1].position) / 100 / 2
        self._axis_length = compute_distance(weels[0].position, weels[2].position) / 100 # cm -> m
        self._max_steer_angle = weels[0].max_steer_angle / 2 # degree 假定内外角一致
        
        self._map = agent.vehicle.get_world().get_map()
        self._world = agent.vehicle.get_world()

        self._target_speed = None
        self.sampling_radius = None
        self._min_distance = None
        self._current_waypoint = None
        self.target_road_option = None
        self._next_waypoints = None
        self.target_waypoint = None
        self._vehicle_controller = None
        self._global_plan = None
        self._pid_controller = None
        
        self.waypoints_queue = deque(maxlen=20000)  # queue with tuples of (waypoint, RoadOption) 10 km
        self._buffer_size = math.ceil(50 / agent._sampling_resolution) # 0.5m res 50m/0.5 向上取整
        self._waypoint_buffer = deque(maxlen = self._buffer_size)
        self._init_controller()  # initializing controller
        self.next_speed = -1

    def get_vehicle_frontcenter(self):
        """
        @param NONE
        @return location:Carla 3D vector
        """
        current_transform = self._vehicle.get_transform()
        location = current_transform.location
        yaw = current_transform.rotation.yaw * math.pi / 180
        location.x += math.cos(yaw) * self._axis_length / 2
        location.y += math.sin(yaw) * self._axis_length / 2
        return location

    def reset_vehicle(self):
        """Reset the ego-vehicle"""
        self._vehicle = None
        print("Resetting ego-vehicle!")

    def _init_controller(self):
        """
        Controller initialization.

        dt -- time difference between physics control in seconds.
        This is can be fixed from server side
        using the arguments -benchmark -fps=F, since dt = 1/F

        target_speed -- desired cruise speed in km/h

        min_distance -- minimum distance to remove waypoint from queue

        lateral_dict -- dictionary of arguments to setup the lateral PID controller
                            {'K_P':, 'K_D':, 'K_I':, 'dt'}

        longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller
                            {'K_P':, 'K_D':, 'K_I':, 'dt'}
        """
        # Default parameters
        self.args_lat_hw_dict = {
            'K_P': 0.75,
            'K_D': 0.02,
            'K_I': 0.4,
            'dt': 1.0 / self.FPS}
        self.args_lat_city_dict = {
            'K_P': 0.58,
            'K_D': 0.02,
            'K_I': 0.5,
            'dt': 1.0 / self.FPS}
        self.args_long_hw_dict = {
            'K_P': 0.37,
            'K_D': 0.024,
            'K_I': 0.032,
            'dt': 1.0 / self.FPS}
        self.args_long_city_dict = {
            'K_P': 0.15,
            'K_D': 0.05,
            'K_I': 0.07,
            'dt': 1.0 / self.FPS}

        self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())

        self._global_plan = False

        self._target_speed = self._vehicle.get_speed_limit()

        self._min_distance = self._sampling_resolution * 2

    def set_speed(self, speed):
        """
        Request new target speed.

            :param speed: new target speed in km/h
        """
        self._target_speed = speed

    def set_global_plan(self, current_plan):
        """
        Sets new global plan.

            :param current_plan: list of waypoints in the actual plan
        """
        for elem in current_plan:
            self.waypoints_queue.append(elem)
        self._global_plan = True

    def get_incoming_waypoint_and_direction(self, steps=3):
        """
        Returns direction and waypoint at a distance ahead defined by the user.

            :param steps: number of steps to get the incoming waypoint.
        """
        if len(self.waypoints_queue) > steps:
            return self.waypoints_queue[steps]

        else:
            try:
                wpt, direction = self.waypoints_queue[-1]
                return wpt, direction
            except IndexError as i:
                print(i)
                return None, RoadOption.VOID
        return None, RoadOption.VOID

    def run_step(self, target_speed=None, debug=True):
        """
        https://blog.csdn.net/u013468614/article/details/103518011
        δ(k)=θe(k)+arctan(λe(k)/v(k))，
        其中，θe(k)为k时刻的航向角偏差，计算方法：目标点方向 - 按现在控制参数车辆到达点的方向
        e(k)为根据上图几何学计算的横向跟踪误差，计算方法：按照当前控制参数车辆到达点 与 目标点 之间的距离
        λ为需要调节的参数，v为无人车当前速度，δ(k)为前轮偏角
        需要注意的是：因为方向盘转角速度是有限制的，因此δ(k)的变化率不能太大，却δ(k)是有最大值的

        stanley的思想是：当我以delta(k)角运动时，我可以完全补偿完所有的横向误差（假定到达点和当前点的朝向一致，即在一条直线上）

        带前馈的stanly模型：
        1.计算当前位置上的横向误差
        2.根据stanley算法计算前轮转角 delta0
        3.计算dt能到达的前馈点
        4.计算前馈点朝向与当前点朝向的差 delta1
        """
        control = carla.VehicleControl(steer=0, throttle=0) # init

        # if target_speed is not None:
        #     self._target_speed = target_speed
        # elif self.next_speed != -1:
        #     self._target_speed = self.next_speed
        # else:
        #     self._target_speed = self._vehicle.get_speed_limit()
        if len(self.waypoints_queue) == 0: # 达到终点
            control = carla.VehicleControl()
            control.steer = 0.0
            control.throttle = 0.0
            control.brake = 1.0
            control.hand_brake = False
            control.manual_gear_shift = False
            return control

        # Buffering the waypoints
        if not self._waypoint_buffer: # 空
            for i in range(self._buffer_size):
                if self.waypoints_queue:
                    self._waypoint_buffer.append(
                        self.waypoints_queue.popleft())
                else:
                    break            

        front_location = self.get_vehicle_frontcenter() # 前轮中心
        current_index = get_nearest_waypoint_index(self._waypoint_buffer, front_location) # 得到
        self._current_waypoint, _ = self._waypoint_buffer[current_index]
        draw_waypoints(self._vehicle.get_world(), [self._current_waypoint], z=2.5) # 
        car_transform = self._vehicle.get_transform()
        car_transform.location = front_location

        # 获取本车当前状态
        speed = get_speed(self._vehicle) # 速度
        acc =get_acceleration(self._vehicle) # 加速度
        current_steer = self._vehicle.get_control().steer # 当前转角比例
        draw_speed_with_tf(self._vehicle.get_world(), car_transform, speed=speed, color=carla.Color(0,255,0), z=2.5) #

        # 计算当前状态下可能到达的target点，因为选择最近点没啥意义，可能你控制就直接过去了
        dist = speed*self._dt + acc / 2.0 * self._dt * self._dt + self._axis_length / 2
        ind = math.ceil(dist / self._sampling_resolution) # 向上取整
        if ind > len(self._waypoint_buffer):
            ind = -1
        self.target_waypoint, self.target_road_option = self._waypoint_buffer[ind]

        # 获取本车位置
        yaw = self._vehicle.get_transform().rotation.yaw * math.pi / 180.0
        
        # 初始化stanley横向控制器
        stanley_controller = StanleyController(front_location, yaw=yaw, v=speed, dt = self._dt, k=0.3)
        # 计算控制角度
        next_steer, lat_dist_error = stanley_controller.run_step(self._current_waypoint, self._max_steer_angle)
        if next_steer > 1 or next_steer < -1 or lat_dist_error > 1:
            self.next_speed = max(speed / 2, 3)
        else:
            self.next_speed = min(target_speed, self.next_speed + 1)
        next_steer = np.clip(next_steer, -1, 1)
        d_steer = 0.05
        if next_steer > current_steer + d_steer:
            next_steer = current_steer + d_steer
        elif next_steer < current_steer - d_steer:
            next_steer = current_steer - d_steer

        if self.next_speed == -1:
            self._target_speed = 10
        else:
            self._target_speed = self.next_speed
        print('next_speed = \033[1;35m {} \033[0m, target_speed={}'.format(self.next_speed, self._target_speed))

        # pid参数选择
        if target_speed > 50:
            args_lat = self.args_lat_hw_dict
            args_long = self.args_long_hw_dict
        else:
            args_lat = self.args_lat_city_dict
            args_long = self.args_long_city_dict

        self._pid_controller = VehiclePIDController(self._vehicle,
                                                    args_lateral=args_lat,
                                                    args_longitudinal=args_long) # 横向pid参数，纵向pid参数

        control = self._pid_controller.run_step(self._target_speed, self.target_waypoint)

        #
        control.steer = next_steer
        car_transform.rotation.yaw += next_steer*self._max_steer_angle
        draw_speed_with_tf(self._vehicle.get_world(), car_transform, speed=speed, color=carla.Color(255,255,0), z=2.5) #

        # Purge the queue of obsolete waypoints
        if current_index > 0:
            for i in range(current_index):
                self._waypoint_buffer.popleft()

                # 增加buffer taosheng 6.11
                if self.waypoints_queue:
                    self._waypoint_buffer.append(
                        self.waypoints_queue.popleft())

        if debug: # 显示局部规划路径
            wps = list(zip(*self._waypoint_buffer))[0]
            if wps:
                draw_way(self._world, wps, time=0.1, thickness=0.5, color=carla.Color(0,0,255))

        return control

class VehicleState():
    def __init__(self, car_location=carla.Location(0,0,0), yaw=0.0, v=0.0):
        self.location  = car_location
        self.v = v
        self.yaw = yaw

    def update(self, acc, delta, axis_l, dt):
        self.location.x += self.v * math.cos(self.yaw) * dt
        self.location.y += self.v * math.sin(self.yaw) * dt
        self.yaw += self.v / axis_l * math.tan(delta) * dt
        self.v += acc * dt

class StanleyController():
    def __init__(self, car_location=carla.Location(0,0,0), yaw=0, v=0, dt=0.01, k=0.3):
        """
        @param car_location : carla.Location
        @param yaw : yaw of vehicle
        @param v : speed of vehicle
        @param dt : period of control
        @param k : parameter of stanley control
        """
        self._dt = dt
        self._k = k
        self._vehicle_state = VehicleState(car_location, yaw, v)
        pass

    def run_step(self, waypoint, max_steer_angle = 35.0):
        """
        @param
        """
        #此路线节点期望的航向角减去当前车辆航向角(航向偏差)，然后再加上横向偏差角即match.atan()得到的值
        #得到的delta即为控制车辆方向盘的控制量 
        wp_yaw = waypoint.transform.rotation.yaw*math.pi/180.0 # 道路朝向，即期望朝向

        car_yaw = self._vehicle_state.yaw # 车辆当前朝向
        
        # 计算横向误差(下一路点处的误差)
        error, angle = compute_magnitude_angle(waypoint.transform.location, self._vehicle_state.location, self._vehicle_state.yaw)

        # 如果路点在车辆左侧，误差为正，反之为负
        # 计算车辆前轴中心到路点的角度
        angle = math.atan2(waypoint.transform.location.y - self._vehicle_state.location.y, waypoint.transform.location.x - self._vehicle_state.location.x) # -pi ~ pi
        diff_angle = angle - car_yaw
        if diff_angle >= math.pi:
            diff_angle -= 2*math.pi
        elif diff_angle < -math.pi:
            diff_angle += 2*math.pi
        if 0> diff_angle > -math.pi:
            error = -1.0*error
        print('error_dist={}, speed={}'.format(error, self._vehicle_state.v))

        if self._vehicle_state.v > 0.1:
            error_yaw = math.atan2(self._k*error, self._vehicle_state.v)
        else:
            error_yaw = 0

        delta = wp_yaw - car_yaw +  error_yaw
        while delta > math.pi:
            delta -= math.pi * 2.0
        while delta < -math.pi:
            delta += math.pi * 2.0

        print('wp_yaw={}, car_yaw={}, error_yaw={}, delta={}'.format(
            math.degrees(wp_yaw), 
            math.degrees(car_yaw), 
            math.degrees(error_yaw), 
            math.degrees(delta)))
        print('')

        steer = delta * 180.0 / math.pi / max_steer_angle
        return steer, error
